import threading
import time
from mqtt import MqttClient
from gyro import Gyro
from myserial import MySerial
from gps import Gps
import json
import HiwonderSDK.Board as Board
import rtmp
from controller import Controller


gyro = Gyro()
controller = Controller(gyro)
client = MqttClient("101.132.123.218",19001,"topic/unmannedship/cmd",controller)
mySerial = MySerial(controller)
myGps = Gps()

threadRunning=True

def setBuzzer(timer):
    Board.setBuzzer(0)
    Board.setBuzzer(1)
    time.sleep(timer)
    Board.setBuzzer(0)

def mqttRun():
    client.start()


def gyroRun():
    while(threadRunning):
        data = gyro.getGryoData()
        #print(str(data[2])+"-"+str(data[5]))
        client.sendGyroData(json.dumps(data))
        time.sleep(0.4)

    print('###Gyro Thread Finished!!!')


def serialRun():
    while(threadRunning):
        mySerial.receive_data()
        
def getGpsDataRun():
    while(threadRunning):
        data = myGps.getGpsData()
        client.sendGpsData(json.dumps(data))
        time.sleep(1)

def gpsRun():
    while(threadRunning):
        myGps.receive_data()

        


def start():
    threadRunning = True
    # 开启mqtt线程
    mqttThread = threading.Thread(target=mqttRun,daemon=True)
    mqttThread.start()

    # 开启gyro线程
    gyroThread = threading.Thread(target=gyroRun,daemon=True)
    gyroThread.start()

    # 串口线程
    serialThread = threading.Thread(target=serialRun,daemon=True)
    serialThread.start()
    
    # gps线程
    #gpsThread = threading.Thread(target=gpsRun,daemon=True)
    #gpsThread.start()

    # gps send线程
    gpsSendThread = threading.Thread(target=getGpsDataRun,daemon=True)
    gpsSendThread.start()
    
    #setBuzzer(1)

    while(threadRunning):

        try:
            time.sleep(1)
            client.heatBeat()
            volt = Board.getBattery()/1000.0
            client.sendVolt(json.dumps([volt,0]))
        except KeyboardInterrupt:
            print('Stop UnmannedShip....')
            threadRunning = False
            break


if __name__ == '__main__':
    start()